
#include "full_cover_path_planner/coord_convert.h"
#include "common/coord_trans.h"
#include <utility>

using namespace global_path_planner::common;

namespace global_path_planner {
namespace ccpp {

    CoordConvert::CoordConvert(){

    }

    CoordConvert::~CoordConvert(){
        
    }

// 轮廓顶点转化为实际值
std::pair<float, float> CoordConvert::Point_2ToReal(Point_2& point_2, float& resolution, 
                                                    double& origin_x, double& origin_y, 
                                                    uint32_t height){
    std::pair<float, float> p = std::make_pair(CGAL::to_double(point_2.x()), CGAL::to_double(point_2.y()));
    auto res_pt = coordtrans::Pixelxy2Realxy(p, resolution, origin_x, origin_y);
    return res_pt;
}

std::vector<std::pair<float, float>> CoordConvert::Point_2sToReals(std::vector<Point_2>& p2_pts, float& resolution, 
                                                                    double& origin_x, double& origin_y, 
                                                                    uint32_t height){
    std::vector<std::pair<float, float>> poly2reals;
    for(auto iter = p2_pts.begin(); iter != p2_pts.end(); iter++){
        poly2reals.emplace_back(Point_2ToReal(*iter, resolution,    origin_x, origin_y, height));
    }
    return poly2reals;
}

// 像素转化为实际值
std::pair<float, float> CoordConvert::cvPoint2Real(cv::Point& cv_point, float& resolution, 
                                                double& origin_x, double& origin_y, uint32_t height){
    std::pair<float, float> p = std::make_pair(cv_point.x, cv_point.y);
    auto res_pt = coordtrans::Pixelxy2Realxy(p, resolution, origin_x, origin_y);
    return res_pt;
}

std::vector<std::pair<float, float>> CoordConvert::cvPoints2Reals(std::vector<cv::Point>& cv_points, float& resolution, 
                                                                            double& origin_x, double& origin_y, uint32_t height){
    std::vector<std::pair<float, float>> cvPoints2reals;
    for(auto iter = cv_points.begin(); iter != cv_points.end(); iter++){
        cvPoints2reals.emplace_back(cvPoint2Real(*iter, resolution, origin_x, origin_y, height));
    }
    return cvPoints2reals;
}

// Vec2d转化为实际值
std::pair<float, float> CoordConvert::Vec2dToReal(Vec2d& vec2d, float& resolution, 
                                                double& origin_x, double& origin_y, 
                                                uint32_t height){
    std::pair<float, float> p = std::make_pair(vec2d.x(), vec2d.y());
    auto res_pt = coordtrans::Pixelxy2Realxy(p, resolution, origin_x, origin_y);
    return res_pt;
}

std::vector<std::pair<float, float>> CoordConvert::Vec2dsToReals(std::vector<Vec2d>& vec2ds, float& resolution, 
                                                                double& origin_x, double& origin_y, uint32_t height){
    std::vector<std::pair<float, float>> vec2ds2reals;
    for(auto iter = vec2ds.begin(); iter != vec2ds.end(); iter++){
        vec2ds2reals.emplace_back(Vec2dToReal(*iter, resolution, origin_x, origin_y, height));
    }
    return vec2ds2reals;
}

}  // namespace ccpp
}  // namespace global_plan_path